import math

from mujoco_py import load_model_from_path, MjSim, MjViewer
import numpy as np

model = load_model_from_path("singleJoint.xml")
sim = MjSim(model, nsubsteps=20)
viewer = MjViewer(sim)
t = 0
while True:
    t += 1
    ctrl_action = np.array([math.cos(t / 10.) * 1])
    print(ctrl_action)
    if sim.data.ctrl is not None:
        for i in range(ctrl_action.shape[0]):
            if sim.model.actuator_biastype[i] == 0:
                sim.data.ctrl[i] = ctrl_action[i]
            else:
                idx = sim.model.jnt_qposadr[sim.model.actuator_trnid[i, 0]]
                sim.data.ctrl[i] = sim.data.qpos[idx] + ctrl_action[i]
    sim.step()
    viewer.render()